import rclpy
from std_msgs.msg import Float64MultiArray
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy


# Topic naming convention:
# /ugv/[sensors | actuators]/device_name/[out | in]/msg_name
# This defines the structure for ROS topics used in the UGV interface.
class UGVinterface:
    """Interface for controlling a vehicle in offboard mode."""

    def __init__(self, main_node: Node = None) -> None:
        self.main_node = main_node

        # Configure QoS profile for publishing and subscribing ??????

        self.qos_profile = 1
        # Create subscribers

        self.chassis_motor_sub = main_node.create_subscription(
            Float64MultiArray,
            "/ugv/actuators/chassis/out/motor_data",
            self.motor_msg_callback,
            qos_profile=self.qos_profile,
        )
        self.chassis_steer_sub = main_node.create_subscription(
            Float64MultiArray,
            "/ugv/actuators/chassis/out/steer_data",
            self.steer_msg_callback,
            qos_profile=self.qos_profile,
        )
        self.lla_sub = main_node.create_subscription(
            Float64MultiArray,
            "/ugv/sensors/rtk/out/lla",
            self.lla_msg_callback,
            qos_profile=self.qos_profile,
        )

        # Create publishers
        self.chassis_pub = main_node.create_publisher(
            Float64MultiArray,
            "/ugv/actuators/chassis/in/to_can_bus",
            qos_profile=self.qos_profile,
        )

        self.motor_data = None
        self.steer_data = None
        self.lla_data = None

    def motor_msg_callback(self, msg: Float64MultiArray) -> None:
        """
        Callback function for motor message.
        """
        print("#DEBUG###")
        self.motor_data = msg.data

    def steer_msg_callback(self, msg: Float64MultiArray) -> None:
        """
        Callback function for steering message.
        """
        self.steer_data = msg.data

    def lla_msg_callback(self, msg: Float64MultiArray) -> None:
        """
        Callback function for LLA message.
        """
        self.lla_data = msg.data

    def publish_chassis(self, mode, shift, speed, steer_angle, brk) -> None:
        """
        Publish chassis data
        """
        msg = Float64MultiArray()
        msg.data.mode = mode
        msg.data.shift = shift
        msg.data.speed = speed
        msg.data.steer = steer_angle
        msg.data.brk = brk
        # Publish the message
        self.chassis_pub.publish(msg)
        self.main_node.get_logger().debug(f"publish chassis data: {msg.data}")
